
#include "odom_model.h"
#define RESOLUTION 0.01

OdomModel::OdomModel(ros::NodeHandle& nh, ros::NodeHandle& ph, boost::mt19937& g)
:n(nh), pn(ph), gaussian(g, boost::normal_distribution<> (0.0, 1.0))
{
	pn.param("alpha_1", a1, 0.25);
	pn.param("alpha_2", a2, 0.25);
	pn.param("alpha_3", a3, 0.25);
	pn.param("alpha_4", a4, 0.25);

	ROS_INFO("Motion model with parameters: a1=%.3lf, a2=%.3lf, a3=%.3lf, a4=%.3lf", a1, a2, a3, a4);
}

void OdomModel::updateMotion(geometry_msgs::PoseArray& particles,
							 double prev_x, double prev_y, double prev_yaw,
							 double next_x, double next_y, double next_yaw)
{
//	static boost::normal_distribution<> gaussian(0.0, 1.0);

	double diff_x = RESOLUTION* (next_x - prev_x);
	double diff_y = RESOLUTION* (next_y - prev_y);
	double del_rot1, del_tran, del_rot2, hat_rot1, hat_tran, hat_rot2;
	// TODO: in place rotation might cause some problem
	double angle = diff_y > 0.01 ? atan2(diff_y, diff_x) : prev_yaw;
	del_rot1 = angle_diff(angle, prev_yaw);
	del_tran = sqrt(diff_x*diff_x + diff_y*diff_y);
	del_rot2 = angle_diff(next_yaw - prev_yaw, del_rot1);

	// TODO: backward rotation?
	const double std_rot1 = sqrt(a1*del_rot1*del_rot1+a2*del_tran*del_tran);
	const double std_tran = sqrt(a3*del_tran*del_tran+a4*(del_rot1*del_rot1+del_rot2*del_rot2));
	const double std_rot2 = sqrt(a1*del_rot2*del_rot2+a2*del_tran*del_tran);

	for(size_t i=0; i< particles.poses.size(); ++i)
	{
		// generate random noise to each parameters
		double err_rot1 = std_rot1 * gaussian();
		double err_tran = std_tran * gaussian();
		double err_rot2 = std_rot2 * gaussian();

//		ROS_INFO("d1 %7.4lf dt %7.4lf d2 %7.4lf", del_rot1, del_tran, del_rot2);
//		ROS_INFO("s1 %7.4lf st %7.4lf s2 %7.4lf", std_rot1, std_tran, std_rot2);
//		ROS_INFO("e1 %7.4lf et %7.4lf e2 %7.4lf ",err_rot1, err_tran, err_rot2);

		hat_rot1 = del_rot1 - err_rot1;
		hat_tran = del_tran - err_tran;
		hat_rot2 = del_rot2 - err_rot2;

//		ROS_INFO("p[%d]@ %.4lf, %.4lf, %.4lf", i, hat_rot1, hat_tran,hat_rot2);
		geometry_msgs::Quaternion& q = particles.poses[i].orientation;
		double heading = atan2(2*q.w*q.z, 1-2*q.z*q.z);
		// update the particle
		particles.poses[i].position.x += hat_tran * cos(heading + hat_rot1);
		particles.poses[i].position.y += hat_tran * sin(heading + hat_rot1);
		heading += (hat_rot1 + hat_rot2);

//		ROS_INFO("p[%d]@ %.4lf, %.4lf, %.4lf, %.4lf", i, particles.poses[i].position.x, particles.poses[i].position.y, particles.poses[i].position.z, heading);

		particles.poses[i].orientation = tf::createQuaternionMsgFromYaw(heading);
	}
}
